clc
clear

% Test1: rpy2tr
fprintf("Test1: rpy2tr(10,20,30)\n");
disp(rpy2tr(10,20,30));

fprintf("Test1: rpy2tr(0,0,30)\n");
disp(rpy2tr(0,0,30));

fprintf("Test2: Plot Tranformered Frame\n");
% Test2: trplot
trplot(rpy2tr(90,90,90));

% Test3: tr2rpy 9
fprintf("Test3: ROLL, PITCH, YAW\n");
T = rpy2tr(10,20,30);
disp(tr2rpy(T)*180/pi);

% Test4: rotx
fprintf("Test4: rotx\n");
disp(rotx(90));

% Test5: roty
fprintf("Test5: roty\n");
disp(roty(90));

% Test6: rotz
fprintf("Test6: rotz\n");
disp(rotz(90));

% Test7: tranimate
fprintf("Test7: tranimate\n");
tranimate(rpy2tr(10,20,30));